/*
* Call includes for the message and constants file
*/
includes Custom;
includes Constants;

module AccelMoteM {
  provides {
    interface StdControl;
  }
  uses {
    interface Timer;
    interface Leds;
    interface StdControl as CommControl;
    interface SendMsg as Send;
	interface ReceiveMsg as Recv;
  }
}
implementation {

  /*
   ** Value of who to send to next which is set at runtime. Cost is also established
  */

  
  /*
	** The Data to actually send
  */
  uint8_t payloadX = 0xB7;
  uint8_t payloadY = 0xC7;
  
  
  struct TOS_Msg sendData;
  struct CustomMsg *message;
  bool pending;
  int state;


  /**
   * Initialize variables and other interfaces
   **/
  command result_t StdControl.init() {
    call Leds.init();
    call CommControl.init();
    pending = FALSE;
    state = 0;
	message = (struct CustomMsg *)sendData.data;
    return SUCCESS;
  }


  /**
   * Start the Timer and CommControl interfaces
   **/
  command result_t StdControl.start() {
    call CommControl.start();
	
    //Start a repeating timer
    call Timer.start(TIMER_REPEAT, TIMER_FIRE_RATE);
	
    return SUCCESS;
  }

  
  /**
   * Halt execution of the application.
   **/
  command result_t StdControl.stop() {
    call CommControl.stop();
    call Timer.stop();
    return SUCCESS;
  }

  /**
   * Send data over the radio 
   **/
  task void sendTask()
  {
    state++;
	
    atomic {
	  message->source = TOS_LOCAL_ADDRESS;
	  message->destination = nextNode;
	  message->cost = cost;
	  message->msgtype = REGULAR_MSG;
      message->pos_x = payloadX;
	  message->pos_y = payloadY;
    }
	
    call Send.send(nextNode, sizeof(struct CustomMsg), &sendData);
  }

  
  /**
   * Event handler for the sendDone event.
   * Required because this module uses the SendMsg interface.
   **/
  event result_t Send.sendDone(TOS_MsgPtr msg, result_t success)
  {
    pending = FALSE;
    dbg(DBG_AM, "sendDone success, Pending is now %i", (int)pending);
	
	//Turn off all leds
	call Leds.redOff();
	call Leds.yellowOff();
	call Leds.greenOff();
	
    return SUCCESS;
  }

  
  /**
   * Each time a timer event is fired, broadcast a message
   * Toggle the red LED to verify this on the mote.
   **/
  event result_t Timer.fired()
  {
	call Leds.redToggle();
	
    post sendTask();
	
	
	return SUCCESS;
  }


  
  event TOS_MsgPtr Recv.receive(TOS_MsgPtr m)
  {
	//Convert the message to our custom type
	CustomMsg *rMsg = (CustomMsg *)m->data;

	//Check if this is an admin message
	if(rMsg->msgtype == ADMIN_MSG)
	{
		//Setup the correct data
		uint8_t tmpNextNode = rMsg->pos_x;
		uint8_t tmpCost = rMsg->pos_y;
		
		//If the cost of this new node is less than our current cost, update our sending node
		if(tmpCost < cost)
		{
		
			//Toggle yellow to show update
			call Leds.yellowToggle();
			
			//Update values accordingly
			nextNode = tmpNextNode;
			cost = tmpCost;
		}
	}
	//If this is a regular message, forward it
	else if(rMsg->msgtype == REGULAR_MSG)
	{
		//Get the messages data 
		payloadX = rMsg->pos_y;
		payloadY = rMsg->pos_x;
		
		
		//Toggle the green light and forward the data
		call Leds.greenToggle();
		post sendTask();
		
	}
	
	return m;
  }
  
}


